#include <Arduino.h>
#include <PulseSensorPlayground.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Constants for pulse sensor
const int PULSE_INPUT = 2;
const int THRESHOLD = 685;

// OLED display dimensions
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET 21
#define SCREEN_ADDRESS 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// PulseSensor object
PulseSensorPlayground pulseSensor;

void setup() {
  Serial.begin(115200);
  delay(1500); 

  // Setup for PulseSensor
  analogReadResolution(10);
  pulseSensor.analogInput(PULSE_INPUT);
  pulseSensor.setSerial(Serial);
  pulseSensor.setThreshold(THRESHOLD);

  if (!pulseSensor.begin()) {
    while (1) {
      Serial.println("PulseSensor initialization failed!");
    }
  }

  // Setup for OLED display
  Wire.begin(17, 18); // SDA, SCL pins for your setup
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1);
  }

  // Display startup logo "DLM"
  displayStartupLogo();
  delay(2000); // Pause for 2 seconds
  display.clearDisplay();
}

void loop() {
  delay(20);

  // Only update display when a beat is detected
  if (pulseSensor.sawStartOfBeat()) {
    float BPM = pulseSensor.getBeatsPerMinute();
    Serial.print("Beat detected! BPM: ");
    Serial.println(BPM);

    // Clear the display
    display.clearDisplay();

    // Display BPM value with alternating colors
    drawBPMWithFlashingColors(BPM);

    // Update the display
    display.display();
  }
}

void drawBPMWithFlashingColors(float BPM) {
  static bool isWhite = true;

  // Toggle color
  isWhite = !isWhite;

  // Set text size and toggle colors
  display.setTextSize(2);
  display.setTextColor(isWhite ? SSD1306_WHITE : SSD1306_BLACK, isWhite ? SSD1306_BLACK : SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print(F("BPM: "));
  display.println(BPM);
}

void displayStartupLogo() {
  display.clearDisplay();

  // Set text size and color for the logo
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  
  // Set cursor to center of the screen
  int16_t x = (SCREEN_WIDTH - 3 * 12) / 2; // Each character is 12 pixels wide in 2x2 text size
  int16_t y = (SCREEN_HEIGHT - 16) / 2;   // Each character is 16 pixels high in 2x2 text size
  
  display.setCursor(x, y);
  
  // Display "DLM"
  display.print(F("D"));
  display.setCursor(x + 12, y);
  display.print(F("L"));
  display.setCursor(x + 24, y);
  display.print(F("M"));
  
  // Update the display to show the logo
  display.display();
}